package gnss

import (
	"dgo/goutils/utils"
	"runtime"
	"sync/atomic"
)

const (
	gnsssv_sig_max byte = 32
)

type GnssSVObs struct {
	NaviSys  byte
	Satid    uint16 // 卫星号
	RoughR   float64
	RoughPRR int16

	PsRngV  [gnsssv_sig_max]float64 // 最终的伪距  单位:米
	PhRngV  [gnsssv_sig_max]float64 // 最终的相位  单位:周
	Doppler [gnsssv_sig_max]float64
	LkT     [gnsssv_sig_max]int64 // 锁定时间, 单位ms
	HalfAmb [gnsssv_sig_max]byte  // Half-cycle ambiguity 半周跳
	Cnr     [gnsssv_sig_max]float32
	LLI     [gnsssv_sig_max]byte //Loss of lock indicator (LLI) For phase observations only
}

func gnsssvobs_finalize(obs *GnssSVObs) {
	atomic.AddInt32(&gnss_sv_obs_cnt, -1)
}

func newGnssSVObs() *GnssSVObs {
	rval := &GnssSVObs{}
	runtime.SetFinalizer(rval, gnsssvobs_finalize)
	atomic.AddInt32(&gnss_sv_obs_cnt, 1)
	return rval
}

func NewGnssSVObsEx(NaviSys byte, Satid uint16) *GnssSVObs {
	rval := gnss_sv_obs_pool.Get().(*GnssSVObs)
	rval.NaviSys = NaviSys
	rval.Satid = Satid
	return rval
}

func FreeGnssSVObs(obj *GnssSVObs) {
	obj.Satid = 0
	obj.RoughPRR = 0
	obj.RoughPRR = 0
	obj.NaviSys = 0
	ResetSVSigListFloat64(&obj.PsRngV)
	ResetSVSigListFloat64(&obj.PhRngV)
	ResetSVSigListFloat64(&obj.Doppler)
	ResetSVSigListInt64(&obj.LkT)
	ResetSVSigListByte(&obj.HalfAmb)
	ResetSVSigListFloat32(&obj.Cnr)
	ResetSVSigListByte(&obj.LLI)
	gnss_sv_obs_pool.Put(obj)
}

type GnssSVObsList struct {
	NaviSys byte
	lst     []*GnssSVObs
}

func NewGnssSVObsList(navisys byte) *GnssSVObsList {
	rval := &GnssSVObsList{NaviSys: navisys}
	atomic.AddInt32(&gnss_sv_obslst_cnt, 1)
	runtime.SetFinalizer(rval, func(obj *GnssSVObsList) {
		atomic.AddInt32(&gnss_sv_obslst_cnt, -1)
	})
	rval.lst = make([]*GnssSVObs, 0, 32)
	return rval
}

func CompGnssSVObsFunc(obs0, obs1 *GnssSVObs) int {
	if obs0.NaviSys == obs1.NaviSys {
		return utils.CompareIntValFunc(int(obs0.Satid), int(obs1.Satid))
	}
	return utils.CompareIntValFunc(int(obs0.NaviSys), int(obs1.NaviSys))
}

func (this *GnssSVObsList) Reset() {
	for i := 0; i < len(this.lst); i++ {
		FreeGnssSVObs(this.lst[i])
	}
	this.lst = this.lst[:0]
	//this.lst = make([]*GnssSVObs, 0, 32)
}

func (this *GnssSVObsList) SatCount() int {
	return len(this.lst)
}

func (this *GnssSVObsList) Range(cb func(e *GnssSVObs) bool) {
	for i := 0; i < len(this.lst); i++ {
		if !cb(this.lst[i]) {
			break
		}
	}
	return
}

func (this *GnssSVObsList) FindObs(satid uint16) *GnssSVObs {
	findIdx, _ := utils.QuickSearchOrInsertIdx(len(this.lst), func(i int) int {
		if satid == this.lst[i].Satid {
			return 0
		} else if satid < this.lst[i].Satid {
			return -1
		} else {
			return 1
		}
	})
	if findIdx == -1 {
		return nil
	} else {
		return this.lst[findIdx]
	}
}

func (this *GnssSVObsList) DeleteFunc(fn func(satid uint16) bool) (n int) {
	for i := len(this.lst) - 1; i > 0; i-- {
		obs := this.lst[i]
		if obs != nil && fn(obs.Satid) {
			this.lst = append(this.lst[:i], this.lst[i+1:]...)
			n++
		}
	}
	return n
}

func (this *GnssSVObsList) Delete(satid uint16) bool {
	idx, _ := utils.QuickSearchOrInsertIdx(len(this.lst), func(i int) int {
		if satid == this.lst[i].Satid {
			return 0
		} else if satid < this.lst[i].Satid {
			return -1
		} else {
			return 1
		}
	})
	if idx == -1 {
		return false
	} else {
		obj := this.lst[idx]
		if obj != nil {
			FreeGnssSVObs(obj)
		}
		this.lst = append(this.lst[:idx], this.lst[idx+1:]...)
		return true
	}
}

func (this *GnssSVObsList) CheckGetObs(satid uint16) *GnssSVObs {
	findIdx, insertIdx := utils.QuickSearchOrInsertIdx(len(this.lst), func(i int) int {
		if satid == this.lst[i].Satid {
			return 0
		} else if satid < this.lst[i].Satid {
			return -1
		} else {
			return 1
		}
	})
	if findIdx == -1 {
		obs := NewGnssSVObsEx(this.NaviSys, satid)
		this.lst = append(this.lst, nil)                   // try to grow the slice in place, any entry works.
		copy(this.lst[insertIdx+1:], this.lst[insertIdx:]) // Move shorter entries down
		this.lst[insertIdx] = obs
		return obs
	} else {
		return this.lst[findIdx]
	}
}

func (this *GnssSVObs) RangeSignal(fn func(signal *GnssSVSignal) bool) {
	var signal GnssSVSignal
	for i := U8_0; i < gnsssv_sig_max; i++ {
		if this.PsRngV[i] != 0 || this.PhRngV[i] != 0 {
			signal.Reset()
			signal.NaviSys = this.NaviSys
			signal.SatId = this.Satid
			signal.SigId = i + 1
			signal.PsV = this.PsRngV[i]
			signal.PhV = this.PhRngV[i]
			signal.Doppler = this.Doppler[i]
			signal.Cnr = this.Cnr[i]
			signal.LkT = this.LkT[i]
			if !fn(&signal) {
				break
			}
		}
	}
}

func (this *GnssSVObs) resetSignal(idx byte) {
	this.PsRngV[idx] = 0
	this.PhRngV[idx] = 0
	this.Doppler[idx] = 0
	this.HalfAmb[idx] = 0
	this.Cnr[idx] = 0
	this.LkT[idx] = 0
	this.LLI[idx] = 0
}

func (this *GnssSVObs) copySignal(signal *GnssSVSignal, idx byte) {
	signal.NaviSys = this.NaviSys
	signal.SatId = this.Satid
	signal.SigId = idx + 1
	signal.PsV = this.PsRngV[idx]
	signal.PhV = this.PhRngV[idx]
	signal.Doppler = this.Doppler[idx]
	signal.Cnr = this.Cnr[idx]
	signal.LLI = this.LLI[idx]
	signal.LkT = this.LkT[idx]
	signal.HalfAmb = this.HalfAmb[idx]
}

func (this *GnssSVObs) ResetRange(fn func(signal *GnssSVSignal) bool) (n int) {
	var signal GnssSVSignal
	for i := U8_0; i < gnsssv_sig_max; i++ {
		if this.PsRngV[i] != 0 || this.PhRngV[i] != 0 {
			signal.Reset()
			this.copySignal(&signal, i)
			if fn(&signal) {
				this.resetSignal(i)
				n++
			}
		}
	}
	return
}

func (this *GnssSVObs) RangeSignalCopy(fn func(signal *GnssSVSignal) bool) {
	for i := U8_0; i < gnsssv_sig_max; i++ {
		if this.PsRngV[i] != 0 || this.PhRngV[i] != 0 {
			signal := &GnssSVSignal{}
			this.copySignal(signal, i)
			if !fn(signal) {
				break
			}
		}
	}
}

func (this *GnssSVObs) GetSingal(sigcode string) (rval *GnssSVSignal) {
	this.RangeSignalCopy(func(signal *GnssSVSignal) bool {
		if signal.GetSignalCode() == sigcode {
			rval = signal
		}
		return rval == nil
	})
	return
}

func (this *GnssSVObs) GetSignalBySigID(sigId byte, signal *GnssSVSignal) {
	if sigId == 0 || sigId > 32 {
		return
	}
	idx := sigId - 1
	signal.NaviSys = this.NaviSys
	signal.SatId = this.Satid
	signal.SigId = sigId
	signal.PsV = this.PsRngV[idx]
	signal.PhV = this.PhRngV[idx]
	signal.Doppler = this.Doppler[idx]
	signal.Cnr = this.Cnr[idx]
	signal.LLI = this.LLI[idx]
	signal.LkT = this.LkT[idx]
}

func (this *GnssSVObs) SetSignal(sig *GnssSVSignal) {
	idx := sig.SigId - 1
	this.PsRngV[idx] = sig.PsV
	this.PhRngV[idx] = sig.PhV
	this.Doppler[idx] = sig.Doppler
	this.Cnr[idx] = sig.Cnr
	this.LkT[idx] = sig.LkT
	this.LLI[idx] = sig.LLI
	this.LkT[idx] = sig.LkT
	this.HalfAmb[idx] = sig.HalfAmb
}

func (this *GnssSVObs) CopyFrom(src *GnssSVObs) {
	this.Satid = src.Satid
	this.NaviSys = src.NaviSys
	this.RoughR = src.RoughR
	for i := U8_0; i < gnsssv_sig_max; i++ {
		this.PsRngV[i] = src.PsRngV[i]
		this.PhRngV[i] = src.PhRngV[i]
		this.Doppler[i] = src.Doppler[i]
		this.LkT[i] = src.LkT[i]
		this.HalfAmb[i] = src.HalfAmb[i]
		this.Cnr[i] = src.Cnr[i]
		this.LLI[i] = src.LLI[i]
	}
}

func (this *GnssSVObs) GetPrnCode() string {
	return GetGnssSvPrnCode(this.NaviSys, this.Satid)
}

func (this *GnssSVObs) CopySig(src, dst byte) {
	this.PhRngV[dst-1] = this.PhRngV[src-1]
	this.PsRngV[dst-1] = this.PsRngV[src-1]
	this.Doppler[dst-1] = this.Doppler[src-1]
	this.LkT[dst-1] = this.LkT[src-1]
	this.HalfAmb[dst-1] = this.HalfAmb[src-1]
	this.Cnr[dst-1] = this.Cnr[src-1]
	this.LLI[dst-1] = this.LLI[src-1]
}

func (this *GnssSVObs) EmptySig(dst byte) {
	this.PhRngV[dst-1] = 0
	this.PsRngV[dst-1] = 0
	this.Doppler[dst-1] = 0
	this.LkT[dst-1] = 0
	this.HalfAmb[dst-1] = 0
	this.Cnr[dst-1] = 0
	this.LLI[dst-1] = 0
}

func (this *GnssSVObs) IsSignalValidIdx(idx byte) bool {
	return this.PsRngV[idx] != 0 || this.PhRngV[idx] != 0
}

func (this *GnssSVObs) IsValid() bool {
	rval := false
	for i := U8_0; i < gnsssv_sig_max; i++ {
		if this.IsSignalValidIdx(i) {
			rval = true
			break
		}
	}

	return rval
}

func (this *GnssSVObs) Write(bb *utils.BytesBuilder) {
	flag := 0
	for i := U8_0; i < 32; i++ {
		if this.PsRngV[i] != 0 || this.PhRngV[i] != 0 {
			if flag == 0 {
				bb.WriteString(this.GetPrnCode())
				bb.WriteString(" ")
				flag = 1
			}
			sigId := i + 1
			bb.WriteString(GnssSvSigId2Code(this.NaviSys, sigId))
			bb.Appendf("[%.2d] ", sigId)
			bb.WriteString(RinexFormatFloat(this.PsRngV[i]))
			bb.WriteString(RinexFormatFloat(this.PhRngV[i]))
			bb.WriteString(RinexFormatFloat(this.Doppler[i]))
			bb.WriteString(RinexFormatFloat(float64(this.Cnr[i])))
			bb.Appendf_PrefixFixed(5, " ", " %d  ", LockTime2Indicator(this.LkT[i]))
			bb.Appendf_PrefixFixed(5, " ", " %d  ", this.HalfAmb[i])
		}
	}

	if flag == 1 {
		bb.WriteString("\r\n")
	}
}
